#include "SE3Math.h"

#define EPSINON 1E-4

void generateZeroMatrix(Matrix* M){
    int i, j;
    for (i = 0; i < 4; i++) {
        for (j = 0; j < 4; j++) {
            M->m[i][j] = 0.0;
        }
    }
    return;
}

void generateDiagonalMatrix(float Diag_value, Matrix* M){
    generateZeroMatrix(M);
    int i;
    for (i = 0; i < 4; i++){
        M->m[i][i] = Diag_value;
    }
    return;
}

void rotateXMatrix(float angle, Matrix* M){
    generateDiagonalMatrix(1.0, M);
    M->m[1][1] = cos(angle);
    M->m[1][2] = -sin(angle);
    M->m[3][1] = sin(angle);
    M->m[3][2] = cos(angle);
}

void rotateYMatrix(float angle, Matrix* M){
    generateDiagonalMatrix(1.0, M);
    M->m[0][0] = cos(angle);
    M->m[0][2] = sin(angle);
    M->m[2][0] = -sin(angle);
    M->m[2][2] = cos(angle);
}

void rotateZMatrix(float angle, Matrix* M){
    generateDiagonalMatrix(1.0, M);
    M->m[0][0] = cos(angle);
    M->m[0][1] = -sin(angle);
    M->m[1][0] = sin(angle);
    M->m[1][1] = cos(angle);
}

void matrixEqual(Matrix* A, Matrix* B){
    int i, j;
    for (i = 0; i < 4; i++){
        for (j = 0; j < 4; j++){
            B->m[i][j] = A->m[i][j];
        }
    }
}

Matrix matrixMultiply(Matrix* A, Matrix* B){
    Matrix M;
    int i, j, k;
    for (i = 0; i < 4; i++) {
        for (j = 0; j < 4; j++) {
            M.m[i][j] = 0;
            for (k = 0; k < 4; k++) {
                M.m[i][j] += A->m[i][k] * B->m[k][j];
            }
        }
    }
    return M;
}

float matrixDetValue(Matrix* M){
    int i, j, k;
    int num = 4;//col or row num; col_num == rownum
    float **data = (float **)malloc(num*sizeof(float*));
    float *pdata = (float *)malloc(num*(num+3)*sizeof(float));
    for(j=0; j<num; j++){
        data[j]=pdata+(num+3)*j;
        for(i=0; i<num; i++) {
            data[j][i] = (float)(M->m[j][i]);
        }
    }
    float value = 1.0;
    for(k=0; k<num; k++){
        if(data[k][k]==0){
            for(j=k+1; j<num; j++){
                if(data[j][k] != 0) {
                    float *buff = data[k]; 
                    data[k] = data[j]; 
                    data[j] = buff; 
                    value   = 0-value; 
                    break;
                }
            }
            if(j==num){
                free(pdata);
                free(data);
                return 0.0f;
            }
        }
        value = value * data[k][k];
        for(j=k+1;j<num;j++){
            float w = data[j][k]/data[k][k];
            for(i=k+1; i<num; i+=4){
                data[j][i  ] -= w*data[k][i ];
                data[j][i+1] -= w*data[k][i+1];
                data[j][i+2] -= w*data[k][i+2];
                data[j][i+3] -= w*data[k][i+3];
            }
        }
    }
    free(pdata);free(data);
    return (float)value;
}

void DH2Matrix(DH dh, float joint, Matrix* M){
    float cos_theta = cos(joint);
    float sin_theta = sin(joint);
    float cos_alpha = cos(dh.alpha);
    float sin_alpha = sin(dh.alpha);

    M->m[0][0] = cos_theta;
    M->m[0][1] = -sin_theta;
    M->m[0][2] = 0;
    M->m[0][3] = dh.a;
    
    M->m[1][0] = sin_theta*cos_alpha;
    M->m[1][1] = cos_theta*cos_alpha;
    M->m[1][2] = -sin_alpha;
    M->m[1][3] = -dh.d*sin_alpha;

    M->m[2][0] = sin_theta*sin_alpha;
    M->m[2][1] = cos_theta*sin_alpha;
    M->m[2][2] = cos_alpha;
    M->m[2][3] = dh.d*cos_alpha;

    M->m[3][0] = 0;
    M->m[3][1] = 0;
    M->m[3][2] = 0;
    M->m[3][3] = 1;

    return;
}

float quatNorm(Quaterion* quat){
    float norm = sqrtf(pow(quat->qx, 2)+pow(quat->qy, 2)+pow(quat->qz, 2)+pow(quat->qw, 2));
    return norm;
}

float quaternionDotProduct(Quaterion* quat1, Quaterion* quat2){
    float result = (quat1->qx * quat2->qx) + (quat1->qy * quat2->qy) + 
                    (quat1->qz * quat2->qz) + (quat1->qw * quat2->qw);
	return result;
}

void quaternionProduct(Quaterion* quat1, Quaterion* quat2, Quaterion* quat){
	quat->qx = (quat1->qw * quat2->qx) + (quat2->qw * quat1->qx) + (quat1->qy * quat2->qz) - (quat2->qy * quat1->qz);
	quat->qy = (quat1->qw * quat2->qy) + (quat2->qw * quat1->qy) + (quat1->qz * quat2->qx) - (quat2->qz * quat1->qx);
	quat->qz = (quat1->qw * quat2->qz) + (quat2->qw * quat1->qz) + (quat1->qx * quat2->qy) - (quat2->qx * quat1->qy);
    quat->qw = (quat1->qw * quat2->qw) - (quat1->qx * quat2->qx) - (quat1->qy * quat2->qy) - (quat1->qz * quat2->qz);
}

void quaternionAdd(Quaterion* quat1, Quaterion* quat2, Quaterion* quat)
{
	quat->qx = quat1->qx + quat2->qx;
	quat->qy = quat1->qy + quat2->qy;
	quat->qz = quat1->qz + quat2->qz;
    quat->qw = quat1->qw + quat2->qw;
	return;
}

void quaternionMultConstant(Quaterion* quat1, float multiplier, Quaterion* quat)
{
	quat->qx = quat1->qx * multiplier;
	quat->qy = quat1->qy * multiplier;
	quat->qz = quat1->qz * multiplier;
	quat->qw = quat1->qw * multiplier;
	return;
}

void quaternionSlerp(Quaterion* start_quat, Quaterion* quat2, float t, Quaterion* quat){
    Quaterion end_quat;
    end_quat.qx = quat2->qx;
    end_quat.qy = quat2->qy;
    end_quat.qz = quat2->qz;
    end_quat.qw = quat2->qw;
	float cos_a = quaternionDotProduct(start_quat, quat2);
    if(cos_a < 0.0f){
        end_quat.qx *= (-1.0f);
        end_quat.qy *= (-1.0f);
        end_quat.qz *= (-1.0f);
        end_quat.qw *= (-1.0f);
        cos_a *= (-1.0f);
    }

	float scale_left = 0.0f;
	float scale_right = 0.0f;
	// incase sina becomes 0.
    if(fabs(cos_a) > (1.0 - EPSINON)){
        scale_left = 1.0 - t;
        scale_right = t;
    }
    else{
        float sin_a = sqrtf(1 - cos_a * cos_a);
        float a = atan2(sin_a, cos_a);
        scale_left = sin((1.0 - t) * a) / sin_a;
        scale_right = sin(t * a) / sin_a;
    }
    quat->qx = (start_quat->qx * scale_left) + (end_quat.qx * scale_right);
    quat->qy = (start_quat->qy * scale_left) + (end_quat.qy * scale_right);
    quat->qz = (start_quat->qz * scale_left) + (end_quat.qz * scale_right);
    quat->qw = (start_quat->qw * scale_left) + (end_quat.qw * scale_right);
    return;
}

void quat2Matrix(Quaterion* quat, Matrix* M){
    generateDiagonalMatrix(1.0, M);
    float norm = quatNorm(quat);
    float qx = quat->qx / norm;
    float qy = quat->qy / norm;
    float qz = quat->qz / norm;
    float qw = quat->qw / norm;
    M->m[0][0] = 1 - 2*(pow(qy, 2)+pow(qz, 2));
    M->m[0][1] = 2*(qx*qy-qz*qw);
    M->m[0][2] = 2*(qx*qz+qy*qw);
    M->m[1][0] = 2*(qx*qy+qz*qw);
    M->m[1][1] = 1 - 2*(pow(qx, 2)+pow(qz, 2));
    M->m[1][2] = 2*(qy*qz-qx*qw);
    M->m[2][0] = 2*(qx*qz-qy*qw);
    M->m[2][1] = 2*(qy*qz+qx*qw);
    M->m[2][2] = 1 - 2*(pow(qx, 2)+pow(qy, 2));
    return;
}

void matrix2Quat(Matrix* M, Quaterion* quat){
    float r11 = M->m[0][0];
    float r12 = M->m[0][1];
    float r13 = M->m[0][2];
    float r21 = M->m[1][0];
    float r22 = M->m[1][1];
    float r23 = M->m[1][2];
    float r31 = M->m[2][0];
    float r32 = M->m[2][1];
    float r33 = M->m[2][2];
    float qx, qy, qz, qw;
    if(r11 + r22 + r33 >= 0.0){
        qw = 0.5 * sqrt(1 + r11 + r22 + r33);
        qx = 0.5 * (r32 - r23) / (2*qw);
        qy = 0.5 * (r13 - r31) / (2*qw);
        qz = 0.5 * (r21 - r12) / (2*qw);
    }
    else if (r11 - r22 - r33 >= 0.0){
        qx = 0.5 * sqrt(1 + r11 - r22 - r33);
        qw = 0.5 * (r32 - r23) / (2*qx);
        qy = 0.5 * (r12 + r21) / (2*qx);
        qz = 0.5 * (r31 + r13) / (2*qx);
    }
    else if (-r11 + r22 - r33 >= 0.0){
        qy = 0.5 * sqrt(1 - r11 + r22 - r33);
        qw = 0.5 * (r13 - r31) / (2*qy);
        qx = 0.5 * (r12 + r21) / (2*qy);
        qz = 0.5 * (r23 + r32) / (2*qy);
    }
        
    else if (-r11 - r22 + r33 >= 0.0){
        qz = 0.5 * sqrt(1 - r11 - r22 + r33);
        qw = 0.5 * (r21 - r12) / (2*qz);
        qx = 0.5 * (r13 + r31) / (2*qz);
        qy = 0.5 * (r23 + r32) / (2*qz);
    }
    quat->qx = qx;
    quat->qy = qy;
    quat->qz = qz;
    quat->qw = qw;
    return;
}

void pose2Matrix(Pose* pose, Matrix* M){
    Quaterion quat; 
    quat.qx = pose->qx;
    quat.qy = pose->qy; 
    quat.qz = pose->qz; 
    quat.qw = pose->qw;
    quat2Matrix(&quat, M);
    M->m[0][3] = pose->x;
    M->m[1][3] = pose->y;
    M->m[2][3] = pose->z;
    return;
}

void matrix2Pose(Matrix* M, Pose* pose){
    pose->x = M->m[0][3];
    pose->y = M->m[1][3];
    pose->z = M->m[2][3];
    Quaterion quat; 
    matrix2Quat(M, &quat);
    pose->qx = quat.qx;
    pose->qy = quat.qy;
    pose->qz = quat.qz;
    pose->qw = quat.qw;
    return;
}

void getQuatFromPose(Pose* pose, Quaterion* quat){
    quat->qx = pose->qx;
    quat->qy = pose->qy;
    quat->qz = pose->qz;
    quat->qw = pose->qw;
}

void getPointFromPose(Pose* pose, Point* point){
    point->x = pose->x;
    point->y = pose->y;
    point->z = pose->z;
}

void euler2Matrix(Euler* euler, Matrix* M){
    //external rotation method and X-Y-Z order for rotation;
    Matrix Rx, Ry, Rz;
    rotateXMatrix(euler->rx_angle, &Rx);
    rotateXMatrix(euler->ry_angle, &Ry);
    rotateXMatrix(euler->rz_angle, &Rz);
    Matrix Rzy = matrixMultiply(&Rz, &Ry);
    Matrix Rzyx = matrixMultiply(&Rzy, &Rx);
    matrixEqual(&Rzyx, M);
    return;
}

void matrix2Euler(Matrix* M, Euler* euler){
    float sy = sqrt(M->m[0][0] * M->m[0][0] + M->m[1][0] * M->m[1][0]);
    int singular = sy < EPSINON ? 1 : 0;
    if(singular){
        euler->rx_angle = atan2(-M->m[1][2], M->m[1][1]);
        euler->ry_angle = atan2(-M->m[2][0], sy);
        euler->rz_angle = 0;
    }
    else{
        euler->rx_angle = atan2(M->m[2][1], M->m[2][2]);
        euler->ry_angle = atan2(-M->m[2][0], sy);
        euler->rz_angle = atan2(M->m[1][0], M->m[0][0]);
    }
    return;
}

void getMiddlePoint(Point* point1, Point* point2, float t, Point* mid_point){
    mid_point->x = (1-t)*point1->x + t*point2->x;
    mid_point->y = (1-t)*point1->y + t*point2->y;
    mid_point->z = (1-t)*point1->z + t*point2->z;
}

void quatPoint2Pose(Quaterion* quat, Point* point, Pose* pose){
    pose->x = point->x;
    pose->y = point->y;
    pose->z = point->z;
    pose->qx = quat->qx;
    pose->qy = quat->qy;
    pose->qz = quat->qz;
    pose->qw = quat->qw;
}